#include <ros/ros.h>
#include <flydatas_msgs/Flydatas.h>
#include <sensor_msgs/NavSatFix.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/State.h>
#include <std_msgs/Header.h>

void flydatasCallback(const flydatas_msgs::Flydatas::ConstPtr& msg) {
    ROS_INFO("Header: Frame ID: %s, Timestamp: %f", msg->header.frame_id.c_str(), msg->header.stamp.toSec());

    ROS_INFO("Flight State: Mode: %s, Armed: %s", msg->flight_state.mode.c_str(), msg->flight_state.armed ? "True" : "False");

    for (int i = 0; i < 4; i++) {
        ROS_INFO("GPS[%d]: Latitude: %f, Longitude: %f, Altitude: %f", i, msg->gps[i].latitude, msg->gps[i].longitude, msg->gps[i].altitude);
    }

    ROS_INFO("Pose: x: %f, y: %f, z: %f", msg->pos.pose.position.x, msg->pos.pose.position.y, msg->pos.pose.position.z);

    ROS_INFO("Do It Flag: %s", msg->doit ? "True" : "False");
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "flydatas_subscriber");
    ros::NodeHandle nh;

    ros::Subscriber sub = nh.subscribe("/pub_flydatas", 10, flydatasCallback);

    ros::spin();

    return 0;
}
